// Simple mathematical formula processing library
//  Written by Atsushi Watanabe
//  Intelligent Robot Laboratory, University of Tsukuba
//
// Copyright 2011 Atsushi Watanabe, All rights reserved.
//
// Redistribution and use in source and binary forms, with or without 
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice, this 
//   list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice, 
//   this list of conditions and the following disclaimer in the documentation 
//   and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE AUTHOR "AS IS" AND ANY EXPRESS OR 
// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO 
// EVENT SHALL THE FREEBSD PROJECT OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 
// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <ctype.h>
#include <sys/time.h>

#include "formula-calc.h"
#include "formula.h"

double math_add( double **val )
{
	return *val[0] + *val[1];
}

double math_sub( double **val )
{
	return *val[0] - *val[1];
}

double math_mul( double **val )
{
	return *val[0] * *val[1];
}

double math_div( double **val )
{
	return *val[0] / *val[1];
}

double math_pow( double **val )
{
	return pow( *val[0], *val[1] );
}

double math_sin( double **val )
{
	return sin( *val[0] );
}

double math_cos( double **val )
{
	return cos( *val[0] );
}

double math_tan( double **val )
{
	return tan( *val[0] );
}

double math_sinh( double **val )
{
	return sinh( *val[0] );
}

double math_cosh( double **val )
{
	return cosh( *val[0] );
}

double math_tanh( double **val )
{
	return tanh( *val[0] );
}

double math_atan( double **val )
{
	return atan( *val[0] );
}

double math_asin( double **val )
{
	return asin( *val[0] );
}

double math_acos( double **val )
{
	return acos( *val[0] );
}

double math_atan2( double **val )
{
	return atan2( *val[0], *val[1] );
}

double math_sqrt( double **val )
{
	return sqrt( *val[0] );
}

double math_exp( double **val )
{
	return exp( *val[0] );
}

double math_let( double **val )
{
	return *val[0] = *val[1];
}

double math_sign( double **val )
{
	if( *val[0] < 0 ) return -1;
	return 1;
}

double math_fabs( double **val )
{
	return 1;
}

double math_log10( double **val )
{
	return log10( *val[0] );
}

double math_ln( double **val )
{
	return log( *val[0] );
}

double math_pi( double **val )
{
	return M_PI;
}

double math_e( double **val )
{
	return M_E;
}

double math_mod( double **val )
{
	return fmod( *val[0], *val[1] );
}

double math_round( double **val )
{
	return round( *val[0] );
}

#define OPERATION_MUL 2
struct operation_t operation[] =
{
	{ "+",     4, TYPE_OP,   math_add,   2 },
	{ "-",     4, TYPE_OP,   math_sub,   2 },
	{ "*",     5, TYPE_OP,   math_mul,   2 },
	{ "/",     5, TYPE_OP,   math_div,   2 },
	{ "^",     7, TYPE_OP,   math_pow,   2 },
	{ "=",     2, TYPE_OP,   math_let,   2 },
	{ "pi",    8, TYPE_MATH, math_pi,    0 },
	{ "e",     8, TYPE_MATH, math_e,     0 },
	{ "log10", 8, TYPE_MATH, math_log10, 1 },
	{ "ln",    8, TYPE_MATH, math_ln,    1 },
	{ "sin",   8, TYPE_MATH, math_sin,   1 },
	{ "cos",   8, TYPE_MATH, math_cos,   1 },
	{ "tan",   8, TYPE_MATH, math_tan,   1 },
	{ "sinh",  8, TYPE_MATH, math_sinh,  1 },
	{ "cosh",  8, TYPE_MATH, math_cosh,  1 },
	{ "tanh",  8, TYPE_MATH, math_tanh,  1 },
	{ "asin",  8, TYPE_MATH, math_asin,  1 },
	{ "acos",  8, TYPE_MATH, math_acos,  1 },
	{ "atan2", 8, TYPE_MATH, math_atan2, 2 },
	{ "atan",  8, TYPE_MATH, math_atan,  1 },
	{ "exp",   8, TYPE_MATH, math_exp,   1 },
	{ "sqrt",  8, TYPE_MATH, math_sqrt,  1 },
	{ "abs",   8, TYPE_MATH, math_fabs,  1 },
	{ "sign",  8, TYPE_MATH, math_sign,  1 },
	{ "round", 8, TYPE_MATH, math_round, 1 },
	{ "mod",   8, TYPE_MATH, math_mod,   2 },
	{ "",     -1, TYPE_MAX , NULL,       0 }
};

struct rpf_t *rpf_last( struct rpf_t *rpf )
{
	for( ; rpf->next; rpf = rpf->next );
	
	return rpf;
}

struct rpf_t *rpf_push( struct rpf_t *rpf, struct stack_t *obj )
{
	rpf->next = malloc( sizeof( struct rpf_t ) );
	rpf->next->type = obj->type;
	rpf->next->value = obj->value;
	rpf->next->next = NULL;
	
	return rpf->next;
}

struct rpf_t *rpf_pushrpf( struct rpf_t *rpf, struct rpf_t *obj )
{
	rpf->next = malloc( sizeof( struct rpf_t ) );
	*rpf->next = *obj;
	if( obj->type == TYPE_VALUE )
	{
		rpf->next->value = malloc( sizeof(double) );
		*((double*)rpf->next->value) = *((double*)obj->value);
	}
	else
	{
		rpf->next->value = obj->value;
	}
	rpf->next->next = NULL;
	
	return rpf->next;
}

struct rpf_t *rpf_join( struct rpf_t *rpf, struct rpf_t *rpf2 )
{
	rpf->next = rpf2;
	for( ; rpf->next; rpf = rpf->next );
	
	return rpf;
}

int rpf_count_num( struct rpf_t *rpf )
{
	int count;
	
	count = 0;
	for( ; rpf; rpf = rpf->next )
	{
		count ++;
	}
	
	return count;
}

struct rpf_t *formula_output( struct stack_t *num, int *sp_num, struct stack_t *op, int *sp_op, int rank )
{
	if( *sp_op == 0 && *sp_num == 1 && num[*sp_num-1].type != TYPE_RPF )
	{
		struct rpf_t rpf2;
		
		rpf2.type = TYPE_START;
		rpf_push( &rpf2, &num[*sp_num-1] );
		(*sp_num) --;
		
		num[*sp_num].rank = 0;
		num[*sp_num].type = TYPE_RPF;
		num[*sp_num].value = rpf2.next;
		(*sp_num) ++;
		
		return NULL;
	}
	if( *sp_op <= 0 ) return NULL;
	
	if( op[*sp_op-1].rank >= rank )
	{
		int j;
		struct rpf_t rpf2, *rpf2_tmp;
		int narg;
		struct rpf_t zero;
		double _zero;
		
		_zero = 0;
		zero.type = TYPE_VALUE;
		zero.value = &_zero;
		
		rpf2_tmp = &rpf2;
		rpf2.type = TYPE_START;
		
		narg = ((struct operation_t*)op[*sp_op-1].value)->narg;
		if( narg > 0 )
		{
			int narg2;

			if( op[*sp_op-1].type == TYPE_MATH )
			{
				narg = 1;
			}
			narg2 = 0;
			for( j = -narg; j < 0; j ++ )
			{
				if( *sp_num + j < 0 )
				{
					rpf2_tmp = rpf_pushrpf( rpf2_tmp, &zero );
				}
				else
				{
					switch( num[*sp_num+j].type )
					{
					case TYPE_RPF:
						rpf2_tmp = rpf_join( rpf2_tmp, num[*sp_num+j].value );
						narg2 ++;
						break;
					default:
						rpf2_tmp = rpf_push( rpf2_tmp, &num[*sp_num+j] );
						narg2 ++;
						break;
					}
				}
			}
			(*sp_num) -= narg2;
		}
		
		rpf2_tmp = rpf_push( rpf2_tmp, &op[*sp_op-1] );
		(*sp_op) --;
		
		return rpf2.next;
	}
	return NULL;
}

int formula( char *expr, struct rpf_t **rpf, struct variables_t *variable )
{
	int i;
	struct stack_t op[ 256 ];
	struct stack_t num[ 256 ];
	int sp_op, sp_num;
	struct rpf_t rpf_start;
	struct rpf_t *rpf_tmp;
	*rpf = NULL;
	int op_cont;
	
	sp_num = 0;
	sp_op = 0;
	op_cont = 0;
	for( ; *expr; expr ++ )
	{
		//printf( "[o%d/n%d] %s\n", sp_op, sp_num, expr );
		if( *expr == '(' )
		{
			char *end;
			int rank2;
			struct rpf_t *rpf2;
			
			rank2 = 0;
			for( ; isspace( *expr ); expr ++ );
			for( end = expr; *end; end ++ )
			{
				if( *end == '(' ) rank2 ++;
				if( *end == ')' ) rank2 --;
				if( rank2 == 0 ) break;
			}
			if( rank2 || !(*end) ) return 0;
			if( formula( expr + 1, &rpf2, variable ) == 0 ) return 0;
			if( rpf2 )
			{
				num[sp_num].rank = 0;
				num[sp_num].type = TYPE_RPF;
				num[sp_num].value = rpf2;
				sp_num ++;
			}
			
			expr = end;
			op_cont = 0;
			continue;
		}
		else if( *expr == ')' )
		{
			op_cont = 0;
			break;
		}
		else if( *expr == ',' )
		{
			while( 1 )
			{
				struct rpf_t *rpf2;
				rpf2 = formula_output( num, &sp_num, op, &sp_op, 0 );
				if( !rpf2 ) break;
				
				num[sp_num].rank = 0;
				num[sp_num].type = TYPE_RPF;
				num[sp_num].value = rpf2;
				sp_num ++;
			}
			op_cont = 0;
			continue;
		}
		else if( isspace( *expr ) )
		{
			op_cont = 0;
			continue;
		}
		for( i = 0; operation[i].type != TYPE_MAX; i ++ )
		{
			if( strstr( expr, operation[i].op ) == expr && 
				( operation[i].type != TYPE_MATH || !isalnum( *( expr + strlen( operation[i].op ) ) ) ) )
			{
				expr += strlen( operation[i].op );
				for( ; isspace( *expr ); expr ++ );
				
				if( sp_num == 0 && operation[i].func == math_sub )
				{
					// Case: start with -
					num[sp_num].rank = 0;
					num[sp_num].type = TYPE_VALUE;
					num[sp_num].value = malloc( sizeof(double) );
					*((double*)num[sp_num].value) = -1;
					sp_num ++;
					op[sp_op].rank = operation[OPERATION_MUL].rank;
					op[sp_op].type = operation[OPERATION_MUL].type;
					op[sp_op].value = &operation[OPERATION_MUL];
					sp_op ++;
					op_cont = 0;
					expr --;
					break;
				}
				else if( op_cont == 1 && operation[i].type != TYPE_MATH )
				{
					if( operation[i].func == math_sub )
					{
						// Case: x*-y
						num[sp_num].rank = 0;
						num[sp_num].type = TYPE_VALUE;
						num[sp_num].value = malloc( sizeof(double) );
						*((double*)num[sp_num].value) = -1;
						sp_num ++;
						op[sp_op].rank = operation[OPERATION_MUL].rank;
						op[sp_op].type = operation[OPERATION_MUL].type;
						op[sp_op].value = &operation[OPERATION_MUL];
						sp_op ++;
						op_cont = 0;
						expr --;
						break;
					}
					else
					{
						// Case: x*/y (Error)
						return 0;
					}
				}
				
				if( operation[i].type != TYPE_MATH )
				{
					op_cont = 1;
				}
				else
				{
					op[sp_op].rank = operation[i].rank;
					op[sp_op].type = operation[i].type;
					op[sp_op].value = &operation[i];
					sp_op ++;
					op_cont = 0;
					if( operation[i].narg > 0 )
					{
						char *end;
						int rank2;
						struct rpf_t *rpf2;
						
						rank2 = 0;
						
						for( end = expr; *end; end ++ )
						{
							if( *end == '(' ) rank2 ++;
							if( *end == ')' ) rank2 --;
							if( rank2 == 0 ) break;
						}
						if( rank2 || !(*end) ) return 0;
						if( formula( expr + 1, &rpf2, variable ) == 0 ) return 0;
						if( !rpf2 ) return 0;
						
						num[sp_num].rank = 0;
						num[sp_num].type = TYPE_RPF;
						num[sp_num].value = rpf2;
						rpf_push( rpf_last( rpf2 ), &op[sp_op-1] );
						sp_op --;
						sp_num ++;
						expr = end + 1;
					}
				}
				if( sp_num > 0 )
				{
					struct rpf_t *rpf3;
					rpf3 = formula_output( num, &sp_num, op, &sp_op, operation[i].rank );
					if( rpf3 )
					{
						num[sp_num].rank = 0;
						num[sp_num].type = TYPE_RPF;
						num[sp_num].value = rpf3;
						sp_num ++;
					}
				}
				if( operation[i].type != TYPE_MATH )
				{
					op[sp_op].rank = operation[i].rank;
					op[sp_op].type = operation[i].type;
					op[sp_op].value = &operation[i];
					sp_op ++;
				}
				expr --;

				break;
			}
		}
		if( operation[i].type != TYPE_MAX ) continue;
		op_cont = 0;
		if( isalpha( *expr ) )
		{
			char *end;
			char variable_name[256];
			int i;
			
			if( !variable ) return 0;
			
			i = 0;
			for( end = expr; *end; end ++ )
			{
				if( !( isalnum( *end ) || *end == '_' || *end == '[' || *end == ']' ) ) break;
				variable_name[ i++ ] = *end;
			}
			variable_name[ i ] = 0;

			for( i = 0; variable[i].name; i ++ )
			{
				if( strcmp( variable[i].name, variable_name ) == 0 )
				{
					num[sp_num].rank = 0;
					num[sp_num].type = TYPE_VARIABLE;
					num[sp_num].value = variable[i].pointer;
					sp_num ++;
					break;
				}
			}
			if( !variable[i].name ) return 0;
			expr = end - 1;
		}
		else
		{
			char *end;
			
			for( end = expr; *end; end ++ )
			{
				if( !( isdigit( *end ) || ( end == expr && *end == '-' ) || *end == '.' ) ) break;
			}
			num[sp_num].rank = 0;
			num[sp_num].type = TYPE_VALUE;
			num[sp_num].value = malloc( sizeof(double) );
			*((double*)num[sp_num].value) = strtod( expr, NULL );
			sp_num ++;
			expr = end - 1;
		}
	}
	while( 1 )
	{
		struct rpf_t *rpf2;
		rpf2 = formula_output( num, &sp_num, op, &sp_op, 0 );
		if( !rpf2 ) break;
		
		num[sp_num].rank = 0;
		num[sp_num].type = TYPE_RPF;
		num[sp_num].value = rpf2;
		sp_num ++;
	}
	if( sp_num < 0 || sp_op != 0 ) return 0;
	
	rpf_start.type = TYPE_START;
	rpf_tmp = &rpf_start;
	for( sp_num --; sp_num >= 0; sp_num -- )
	{
		switch( num[sp_num].type )
		{
		case TYPE_RPF:
			rpf_tmp = rpf_join( rpf_tmp, num[sp_num].value );
			break;
		default:
			rpf_tmp = rpf_push( rpf_tmp, &num[sp_num] );
			break;
		}
	}
	*rpf = rpf_start.next;
	return 1;
}

void formula_free( struct rpf_t *rpf )
{
	for( ; rpf; )
	{
		struct rpf_t *tmp;
		switch( rpf->type )
		{
		case TYPE_VALUE:
			free( rpf->value );
			break;
		case TYPE_VARIABLE:
			break;
		default:
			break;
		}
		tmp = rpf->next;
		free( rpf );
		rpf = tmp;
	}
}

double formula_eval( struct rpf_t *rpf )
{
	double _stack[512];
	double *stack[512];
	int sp;
	struct operation_t *op;
	double tmp;
	
	sp = 0;
	for( ; rpf; rpf = rpf->next )
	{
		switch( rpf->type )
		{
		case TYPE_VALUE:
		case TYPE_VARIABLE:
			stack[sp] = ( (double*)rpf->value );
			sp ++;
			break;
		default:
			op = (struct operation_t*)rpf->value;
			tmp = (op->func)( &stack[ sp - op->narg ] );
			sp -= op->narg;
			stack[sp] = &_stack[sp];
			_stack[sp] = tmp;
			sp ++;
			break;
		}
	}
	return *stack[0];
}

struct rpf_t *formula_optimize( struct rpf_t *rpf )
{
	double _stack[512];
	double *stack[512];
	struct rpf_t _rpf_st[512];
	struct rpf_t *rpf_st[512];
	int fixed[512];
	int sp;
	struct rpf_t *rpf_new;
	struct rpf_t rpf_new_st;
	int i;
	
	sp = 0;
	for( ; rpf; rpf = rpf->next )
	{
		struct operation_t *op;
		double tmp;
		switch( rpf->type )
		{
		case TYPE_VALUE:
			stack[sp] = ( (double*)rpf->value );
			fixed[sp] = 1;
			rpf_st[sp] = rpf;
			sp ++;
			break;
		case TYPE_VARIABLE:
			stack[sp] = ( (double*)rpf->value );
			fixed[sp] = 0;
			rpf_st[sp] = rpf;
			sp ++;
			break;
		default:
			op = (struct operation_t*)rpf->value;
			for( i = 0; i < op->narg; i ++ )
			{
				if( !fixed[sp-1-i] ) break;
			}
			if( i == op->narg )
			{
				tmp = (op->func)( &stack[ sp - op->narg ] );
				sp -= op->narg;
				stack[sp] = &_stack[sp];
				_stack[sp] = tmp;
				fixed[sp] = 1;
				
				rpf_st[sp] = &_rpf_st[sp];
				rpf_st[sp]->type = TYPE_VALUE;
				rpf_st[sp]->value = &_stack[sp];
				sp ++;
			}
			else
			{
				rpf_st[sp] = rpf;
				fixed[sp] = 0;
				sp ++;
			}
			break;
		}
	}
	
	rpf_new = &rpf_new_st;
	rpf_new->type = TYPE_START;
	for( i = 0; i < sp; i ++ )
	{
		rpf_new = rpf_pushrpf( rpf_new, rpf_st[i] );
	}
	formula_free( rpf );
	return rpf_new_st.next;
}

void formula_print( FILE *stream, struct rpf_t *rpf )
{
	for( ; rpf; rpf = rpf->next )
	{
		switch( rpf->type )
		{
		case TYPE_VALUE:
			fprintf( stream, "%f ", *(double*)rpf->value );
			break;
		case TYPE_VARIABLE:
			fprintf( stream, "VARIABLE " );
			break;
		default:
			fprintf( stream, "%s ", ((struct operation_t*)rpf->value)->op );
			break;
		}
	}
}

